classdef pidControler  < handle
    %这个用的是增量式pid控制器
    properties
        u_now;     % 控制量，每次自增增量，成为真正的控制输出
        ref_value;   %这个就是参考值，然后减去实际值就是err
        %需要存的其实只有前两次，当次的可以不用记录，因为又不会被用在外面或者别处
        err_last;   %上次的误差
        err_ll;     %上上次的误差
        err_sum;    %求和的误差，用于增量式
        kp;
        ki;
        kd;
        n_group;
    end
    
    methods
        function obj = pidControler(n_group,kp,ki,kd,ref_value)
            if(nargin == 4)
                obj.n_group = n_group;
                obj.kp = kp;
                obj.ki = ki;
                obj.kd = kd;
                disp("using default value 0");
                obj.ref_value = zeros(n_group,1);
            elseif(nargin == 5)
                obj.n_group = n_group;
                obj.ref_value = ref_value;
                obj.kp = kp;
                obj.ki = ki;
                obj.kd = kd;
                disp("initial fully");
            else
                ME = MException('need at least 3 pid param!');
                throw(ME)
            end
            [obj.u_now,obj.err_last,obj.err_ll,obj.err_sum] = deal(zeros(n_group,1));
        end
        
        
        function pid_reset(obj,kp,ki,kd)
            obj.kp = kp;
            obj.ki = ki;
            obj.kd = kd;
            [obj.u_now,obj.err_last,obj.err_ll,obj.err_sum] = deal(zeros(obj.n_group,1));
        end
        
        function con_value = get_add_up_con_value(obj,actual_value)
            %这个是获得增量式的控制值
            %暂时不用
            err_now = obj.ref_value - actual_value;
            if(err_now < -pi)
                err_now = mod(err_now,pi);
            elseif(err_now > pi)
                err_now = mod(err_now,-pi);
            end
            obj.u_now = obj.u_now + obj.kp * (err_now - obj.err_last) + obj.ki * err_now + obj.kd * (err_now - 2 * obj.err_last + obj.err_ll);            
            obj.err_ll = obj.err_last;
            obj.err_last = err_now;
            con_value = obj.u_now;
        end
        
        function debug_info = get_sum_up_con_value(obj,actual_value)                        
            %这个是获得累加式的控制值
            debug_info = zeros(obj.n_group,1,4);%con, e_n, e_l, e_s
            err_now = obj.ref_value - actual_value;
            
            obj.err_sum = obj.err_sum + err_now ;
            obj.u_now = obj.kp .* err_now + obj.ki .* obj.err_sum + obj.kd .* (err_now - obj.err_last);            
            debug_info(:,1,3) =  obj.err_last;
            obj.err_last = err_now;
            debug_info(:,1,1) = obj.u_now;
            debug_info(:,1,2) = err_now;
            debug_info(:,1,4) = obj.err_sum;
        end
        
        function set_target(obj,ref_value)
            obj.ref_value = ref_value;            
        end
    end
     
end